//
// Created by axi on 22-10-16.
//
#include <opencv2/opencv.hpp>
#include <cmath>
#include "getNeeds.h"
#include "windmill.hpp"

using namespace std;
using namespace cv;

double getNeeds::getCoordinate(cv::Mat &src, int &x1, int &y1, int &x2, int &y2)
{
    //二值化的较低的阈值，因为本身不需要筛掉任何的东西，所以可以为0
    int threshold_v = 0;
    //二值化的较高的阈值，因为本身不需要筛掉任何的东西，所以可以为255
    int threshold_max = 255;
    //创建点的集合以储存轮廓信息
    vector<vector<Point>> contours;
    //储存分层信息，hierarchy.size()就是一共有几个轮廓的意思
    vector<Vec4i> hierarchy;
    //两个画布，gray_src储存原图的灰度图，analysis用于后续的全部处理
    Mat analysis,gray_src;
    Mat element1 = getStructuringElement(MORPH_RECT, Size(1, 1));//设置内核1,用于开操作
    Mat element2 = getStructuringElement(MORPH_RECT, Size(10, 10));//设置内核2,用于闭操作
    cvtColor(src, gray_src, COLOR_BGR2GRAY);//将原图转化为灰度图
    GaussianBlur(gray_src, gray_src, Size(3, 3), 0, 0);//进行高斯滤波模糊边缘
    threshold(gray_src, analysis, threshold_v, threshold_max, THRESH_BINARY_INV);//二值化处理
    floodFill(analysis, Point(0, 0), Scalar(0));//漫水处理，从点(0,0)开始遍历，效果为让轮廓中的黑白颜色翻转
    morphologyEx(analysis, analysis, MORPH_CLOSE, element2);//闭运算，先膨胀后腐蚀，消除已经被击打的击打板的中间缝隙
    findContours(analysis, contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);//获取轮廓，RETR_EXTERNAL为获取外轮廓
    //通过计算轮廓的面积来判断并求出能量机关中心及待击打板中心，其中能量机关中心的轮廓面积最小，待击打板第二小
    int area[6] = {};
    for (int i = 0; i < hierarchy.size(); i++)
    {
        area[i] = contourArea(contours[i]);//获取面积
    }
    for (int i = 0; i < hierarchy.size(); i++) {
        if (area[i] < 1500 && area[i]>100) {//为待击打板
            RotatedRect box1 = minAreaRect(Mat(contours[i]));//获取最小外接矩阵，以获取轮廓中心点坐标
            circle(src, Point(box1.center.x, box1.center.y), 5, Scalar(0, 255, 0), -1, 8);////绘制最小外接矩形的中心点
            x2=box1.center.x;//获取坐标
            y2=box1.center.y;//获取坐标
        }
        if (area[i] < 100) {//为能量机关中心
            RotatedRect box1 = minAreaRect(Mat(contours[i]));//获取最小外接矩阵，以获取轮廓中心点坐标
            circle(src, Point(box1.center.x, box1.center.y), 5, Scalar(110, 110, 110), -1, 8);  //绘制最小外接矩形的中心点
            x1=box1.center.x;//获取坐标
            y1=box1.center.y;//获取坐标
        }
    }
}

double getNeeds::getAlpha(double *getSin, double *getTime) {
    //数据数量
    int num=1000;
    //创建两个数组对主函数的sin1与time1接收
    double sin1[num],time1[num];
    //接收
    for(int i=0;i<num;i++)
    {
        sin1[i]= * getSin;
        time1[i]= * getTime;
        getSin++;
        getTime++;
    }
    //初始化alpha
    double alpha=0;
    //loss用于储存当前的代价函数的值，loss_temp为代价函数临时的一个储存
    double loss=100000,loss_temp;
    //sin2为拟合所用的sin
    double sin2[num];
    //由于alpha为角，所以可以看作其只有由0到2PI的取值范围，故按照需要的精度（我认为我需要小数点后两位的精度）进行遍历即可
    for(double j=0;j<628;j++) {
        for (int i = 0; i < num; i++) {
            sin2[i] = sin(1.884 * time1[i] + j/100);
        }
        loss_temp = 0;
        for (int i = 0; i < num; i++) {
            loss_temp += pow(sin2[i] - sin1[i], 4);
        }
        //如果当前代价函数为新低，记录此时的alpha
        if(loss>loss_temp)
        {
            loss=loss_temp;
            alpha=j/100;
        }
    }
    return alpha;
}

double getNeeds::getVreal(int &a1, int &b1, int &a2, int &b2, int &a3, int &b3, int &a4, int &b4, double &time1, double &time2,double &r) {
    //接收数据
    int x1=a1,y1=b1,x2=a2,y2=b2,x3=a3,y3=b3,x4=a4,y4=b4;
    double t1=time1,t2=time2;
    //求半径r
    r=(sqrt(pow(x1-x2,2)+pow(y1-y2,2))+sqrt(pow(x3-x4,2)+pow(y3-y4,2)))/2;
    //求两组数据之间的时间转过的角度
    double angle_cos=abs((x1-x2)*(x3-x4)+(y1-y2)*(y3-y4))/(sqrt(pow(x1-x2,2)+pow(y1-y2,2))*sqrt(pow(x3-x4,2)+pow(y3-y4,2)));
    //不能让cos的值在-1到1之外，否则进行纠正
    if(angle_cos>1)
        angle_cos=1;
    if(angle_cos<-1)
        angle_cos=-1;
    //取得角度
    double angle= acos(angle_cos);
    //取得v作为角速度
    double v=angle/abs(t2-t1);
    //由于原算式仍有sin存在，(v-1.305)/0.785需要在-1到1之间，否则纠正
    if(v>2.09)
        v=2.09;
    if(v<0.52)
        v=0.52;
    v=(v-1.305)/0.785;
    return v;
}
